#include "pid.h"
#include "motor.h"
#include "tim.h"


// 定义四个电机的PID控制结构体变量
tPid pidMotor1Speed;
tPid pidMotor2Speed;
tPid pidMotor3Speed;
tPid pidMotor4Speed;

// 定义四个电机转速控制的PID结构体变量
tPid pid1_speed; //电机1的转速控制
tPid pid2_speed; //电机2的转速控制
tPid pid3_speed; //电机3的转速控制
tPid pid4_speed; //电机4的转速控制


// 定义电机速度控制的最大值和最小值
float max=3.1;
float min=2.9;

// 定义四个编码器的计数器值变量
short Encoder1Count = 0; //编码器1计数器值
short Encoder2Count = 0;
short Encoder3Count = 0; //编码器3计数器值
short Encoder4Count = 0;


// 定义四个电机的速度变量
float Motor1Speed = 0.00;
float Motor2Speed = 0.00;
float Motor3Speed = 0.00;
float Motor4Speed = 0.00;

// 定义四个电机PWM调速的变量
int motor_pwm1=0;
int motor_pwm2=0;
int motor_pwm3=0;
int motor_pwm4=0;

// 定义定时器计数变量
uint16_t TimerCount=0;

// PID初始化函数，用于设置PID参数和启动定时器
void pid_init(void)
{
    // 启动基础定时器和编码器定时器
    HAL_TIM_Base_Start_IT(&htim1);
    HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL); //开启定时器2
    HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL); //开启定时器4
    HAL_TIM_Encoder_Start(&htim5,TIM_CHANNEL_ALL);
    HAL_TIM_Base_Start_IT(&htim2); //开启定时器2中断
    HAL_TIM_Base_Start_IT(&htim3);
    HAL_TIM_Base_Start_IT(&htim4); 
    HAL_TIM_Base_Start_IT(&htim5);
    HAL_TIM_Base_Start_IT(&htim6);

    // 初始化电机1的PID控制参数
    pidMotor1Speed.actual_val=0.0;
    pidMotor1Speed.target_val=0.00;
    pidMotor1Speed.err=0.0;
    pidMotor1Speed.err_last=0.0;
    pidMotor1Speed.err_sum=0.0;
    pidMotor1Speed.Kp=0;
    pidMotor1Speed.Ki=0;
    pidMotor1Speed.Kd=0;

    // 初始化电机1转速PID结构体
    pid1_speed.actual_val=0.0;
    pid1_speed.target_val=0.0;
    pid1_speed.err=0.0;
    pid1_speed.err_last=0.0;
    pid1_speed.err_sum=0.0;
    pid1_speed.Kp=0.0;
    pid1_speed.Ki=0.0;
    pid1_speed.Kd=0.0;
    
    // 初始化电机2转速PID结构体
    pid2_speed.actual_val=0.0;
    pid2_speed.target_val=0.0;
    pid2_speed.err=0.0;
    pid2_speed.err_last=0.0;
    pid2_speed.err_sum=0.0;
    pid2_speed.Kp=0.0;
    pid2_speed.Ki=0.0;
    pid2_speed.Kd=0.0;

    // 初始化电机3转速PID结构体
    pid3_speed.actual_val=0.0;
    pid3_speed.target_val=0.0;
    pid3_speed.err=0.0;
    pid3_speed.err_last=0.0;
    pid3_speed.err_sum=0.0;
    pid3_speed.Kp=0.0;
    pid3_speed.Ki=0.0;
    pid3_speed.Kd=0.0;
    
    // 初始化电机4转速PID结构体
    pid4_speed.actual_val=0.0;
    pid4_speed.target_val=0.0;
    pid4_speed.err=0.0;
    pid4_speed.err_last=0.0;
    pid4_speed.err_sum=0.0;
    pid4_speed.Kp=0.0;
    pid4_speed.Ki=0.0;
    pid4_speed.Kd=0.0;
}


// 定时器中断回调函数
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  // 如果是定时器1的中断
  if(htim == &htim1) // htim1 500HZ 2ms 中断一次
	{
		TimerCount++;
		if(TimerCount %5 == 0) // 每10ms执行一次
		{  
				 // 读取编码器的计数值并计算电机速度
			Encoder1Count = -(short)__HAL_TIM_GET_COUNTER(&htim2);
			Encoder2Count = (short)__HAL_TIM_GET_COUNTER(&htim3);
			Encoder3Count = -(short)__HAL_TIM_GET_COUNTER(&htim4);
			Encoder4Count = (short)__HAL_TIM_GET_COUNTER(&htim5);
			__HAL_TIM_SET_COUNTER(&htim2,0);
			__HAL_TIM_SET_COUNTER(&htim3,0);
			__HAL_TIM_SET_COUNTER(&htim4,0);
			__HAL_TIM_SET_COUNTER(&htim5,0);
			Motor1Speed = (float)Encoder1Count*0.000935*100/6;   // cm/s
			Motor2Speed = (float)Encoder2Count*0.000935*100/6;
			Motor3Speed = (float)Encoder3Count*0.000935*100/6;
			Motor4Speed = (float)Encoder4Count*0.000935*100/6;
			//           HC_SR04_Read();
			TimerCount=0;
		}
		 //	   if(TimerCount %10 == 0)//每20ms执行一次 
//       {
//           motor_set(PID_realize(&pid1_speed,Motor1Speed),PID_realize(&pid2_speed,Motor2Speed), 
//                     PID_realize(&pid3_speed,Motor3Speed),PID_realize(&pid4_speed,Motor4Speed));
//           ANO_DT_Send_F1(Motor1Speed*100, Motor2Speed*100, Motor3Speed*100, Motor4Speed*100);
//           TimerCount=0;
//       }  
	}
	
    // 如果是定时器6的中断
	if(htim == &htim6) // htim6 500HZ 0.1ms 中断一次
	{
    ////		xunji_start(); 
//        HC_SR04_Read();
//        lanya();  
//         if(HC_SR04_Read()<15)
//           {
//             beep();  
//           }  
	}
}

// 电机速度控制函数
void ABcatch_crol(void)
{
  // 根据电机速度与设定的最大最小值进行PWM调整
  if(Motor1Speed>max) motor_pwm1--;
	if(Motor1Speed<min) motor_pwm1++;
	if(Motor2Speed>max) motor_pwm2--;
	if(Motor2Speed<min) motor_pwm2++;
	if(Motor3Speed>max) motor_pwm3--;
	if(Motor3Speed<min) motor_pwm3++;
	if(Motor4Speed>max) motor_pwm4--;
	if(Motor4Speed<min) motor_pwm4++;
  motor_set(motor_pwm1,motor_pwm2,motor_pwm3,motor_pwm4); // 设置电机PWM值
}


// 比例P调节控制函数
float P_realize(tPid * pid,float actual_val)
{
	pid->actual_val = actual_val; // 传递真实值
	pid->err = pid->target_val - pid->actual_val; // 当前误差=目标值-真实值
	// 比例控制调节 输出=Kp*当前误差
	pid->actual_val = pid->Kp*pid->err;
	return pid->actual_val;
}

// 比例P积分I控制函数
float PI_realize(tPid * pid,float actual_val)
{
	pid->actual_val = actual_val; // 传递真实值
	pid->err = pid->target_val - pid->actual_val; // 当前误差=目标值-真实值
	pid->err_sum += pid->err; // 误差累计值 = 当前误差累计和
	// 使用PI控制 输出=Kp*当前误差+Ki*误差累计值
	pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->err_sum;
	return pid->actual_val;
}

// PID控制函数
float PID_realize(tPid * pid,float actual_val)
{
	pid->actual_val = actual_val; // 传递真实值
	pid->err = pid->target_val - pid->actual_val; // 目标值减去实际值等于误差值
	pid->err_sum += pid->err; // 误差累计求和
	// 使用PID控制 输出=Kp*当前误差 + Ki*误差累计值 + Kd*(当前误差 - 上次误差)
	pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->err_sum + pid->Kd*(pid->err - pid->err_last);
	// 保存上次误差:最近一次 赋值给上次
	pid->err_last = pid->err;
	return pid->actual_val;
}

